%% 文献《到达角定位误差分析》仿真案例复现 两个监测站 %%
clc
clear
close all
% 公式定义见文献《三维多站侧向交叉定位算法及精度分析》 李洪梅
% 该脚本为 main 函数，调用了 gdop_2 函数

sigma_theta = 1*pi/180;% 方位角测量误差
sigma_fi = 1*pi/180;% 高低角测量误差
sigma_s = 0.01; % 站址误差
S1 = [20 0 0];% 监测站1坐标
S2 = [-20 0 0];% 监测站2坐标
K = 100;% 正方形边长
% 辐射源坐标 XYZ
[X,Y] = meshgrid(-K:1:K);
X(:,101) = [];
Y(:,101) = [];
z = 10;
% 条件1
GDOP = gdop_2(K,X,Y,z,sigma_theta,sigma_fi,sigma_s,S1,S2);
figure
contour(X,Y,GDOP,[0.5 0.7 1 2 5 10 20],'ShowText','on');
xlabel('x(km)');ylabel('y(km)');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
% title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);
hold;plot([S1(1),S2(1)],[S1(2),S2(2)],'*','Color','r');
%% 换条件 %%
% 条件2 增加角度误差
sigma_theta = 2*pi/180;
sigma_fi = 2*pi/180;
GDOP = gdop_2(K,X,Y,z,sigma_theta,sigma_fi,sigma_s,S1,S2);
figure
contour(X,Y,GDOP,[1.3 1.5 2 4 6 8 10 20 30 50],'ShowText','on');
xlabel('x(km)');ylabel('y(km)');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
hold;plot([S1(1),S2(1)],[S1(2),S2(2)],'*','Color','r');
% title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);

% 增加方位角侧向误差 sigma_theta = 0.01;sigma_fi = 0.001 时 图不对 
% 高低角和方位角误差 不在一个量级就会出现这种现象

% 条件3 增加站址误差
sigma_theta = 1*pi/180;
sigma_fi = 1*pi/180;
sigma_s = 0.03; 
GDOP = gdop_2(K,X,Y,z,sigma_theta,sigma_fi,sigma_s,S1,S2);
figure
contour(X,Y,GDOP,[0.7 0.8 1 2 4 6 10 20 40],'ShowText','on');
xlabel('x(km)');ylabel('y(km)');
% title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
hold;plot([S1(1),S2(1)],[S1(2),S2(2)],'*','Color','r');
% 条件4 增加测向站之间的距离
sigma_theta = 1*pi/180;
sigma_fi = 1*pi/180;
sigma_s = 0.01; 
S1 = [-60 0 0];% 监测站1坐标
S2 = [60 0 0];% 监测站2坐标
GDOP = gdop_2(K,X,Y,z,sigma_theta,sigma_fi,sigma_s,S1,S2);
figure
contour(X,Y,GDOP,[2.5 3 4 5 10],'ShowText','on');
xlabel('x(km)');ylabel('y(km)');
title(['\sigma_\theta = ',num2str(sigma_theta),...
       ' \sigma_\phi = ',num2str(sigma_fi),...
       ' \sigma_s = ',num2str(sigma_s)]);
% title(['GDOP_m_i_n = ',num2str(min(min(GDOP)))]);
hold;plot([S1(1),S2(1)],[S1(2),S2(2)],'*','Color','r');